#include "aruco_location/read_param.hpp"

double ARUCO_LEN;
int    ARUCO_ID;
int    DRAW_POSE;

void aruco_location::readParameters(const std::string& config_path)
{
    cv::FileStorage fsSettings(config_path, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "\033[1;32m\n-----> ERROR: Wrong path to settings \n \033[0m" << std::endl;
        return;
    }

    fsSettings["aruco_length"] >> ARUCO_LEN;
    ARUCO_ID  = fsSettings["aruco_id"];
    DRAW_POSE = fsSettings["draw_pose"]; 

    // std::cout << "\n\033[1;32m-----> aruco_length: " << ARUCO_LEN << "m\033[0m" << std::endl;
    // std::cout << "\033[1;32m-----> aruco_id: " << ARUCO_ID << "\n \033[0m" << std::endl;

    fsSettings.release();
}